/**************************************************************************
File:        stepper.cpp
Description: Stepper motor class

Copyright:   (C) 1998-2012 by ZJ Laczik
Email:       Zsolt.Laczik@eng.ox.ac.uk
****************************************************************************
Change log:
   begin      19/11/1998
   15/2/2012	QT4 update
***************************************************************************/

#include <time.h>

#include <QApplication>

#include "micromachine.h"
#include "hal.h"

#include "stepper.h"

// 5 us min pulse width -> ~100 kHz max frequency
// and 1Hz - 50kHz frequency range
#define MINPW	5
#define MINFREQ 1
#define MAXFREQ 50000

#define DEFFREQ	100
#define DEFPW	500

extern Hal* gHal;

Stepper::Stepper(QWidget *parent) : QObject(parent) {
    current_pos[X_AX] = current_pos[Y_AX] = current_pos[Z_AX] = 0;

    StopFlag = false ;
    pStopFlag = &StopFlag ;
    
    slotSetMaxFreq(DEFFREQ);
    slotSetMinFreq(DEFFREQ/10);
    slotSetFreqIncr(DEFFREQ/10);
    slotSetPulseWidth(DEFPW);
}

Stepper::~Stepper() {
}

void Stepper::slotSetMinFreq(int freq)
{
  if (freq==mMinFreq) {
    // no change
    return;
  }
  else if (freq<MINFREQ || freq>MAXFREQ || freq>mMaxFreq || (1000000L/freq)<(mPulseWidth+2)) {
    // invalid freq
    ///// should dalso emit a dsp msg here to advise
    emit(sigMinFreqChanged(mMinFreq));
    return;
  }
  else {
    mMinFreq = freq;
    mMaxPeriod = 1000000L/freq;
    emit(sigMinFreqChanged(mMinFreq));
  } 
}

void Stepper::slotSetMaxFreq(int freq)
{
  if (freq==mMaxFreq) {
    // no change
    return;
  }
  else if (freq<MINFREQ || freq>MAXFREQ || freq<mMinFreq || (1000000L/freq)<(mPulseWidth+2)) {
    // invalid freq
    ///// should also emit a dsp msg here to advise
    emit(sigMaxFreqChanged(mMaxFreq));
    return;
  }
  else {
    mMaxFreq = freq;
    mMinPeriod = 1000000L/freq;
    emit(sigMaxFreqChanged(mMaxFreq));
  } 
}

void Stepper::slotSetFreqIncr(int freq)
{
  if (freq==mFreqIncr) {
    // no change
    return;
  }
  else if (freq<1) {
    // invalid freq
    ///// should also emit a dsp msg here to advise
    emit(sigFreqIncrChanged(mFreqIncr));
    return;
  }
  else {
    mFreqIncr = freq;
    mPeriodIncr = (mMinPeriod+mMaxPeriod)*freq/(mMinFreq+mMaxFreq);
    emit(sigFreqIncrChanged(mFreqIncr));
  } 
}

void Stepper::slotSetPulseWidth(long int pw)
{
  if (pw==mPulseWidth) {
    // no change
    return;
  }
  else if (pw<MINPW || pw>(mMinPeriod-2)) {
    // invalid value
    ///// should also emit a dsp msg here to advise
    emit(sigPulseWidthChanged(mPulseWidth));
    return;
  }
  else {
    mPulseWidth = pw;
    emit(sigPulseWidthChanged(mPulseWidth));
  } 
}

void Stepper::SetStopFlagPointer(bool * pSF)
{
    pStopFlag = pSF ;
}

void Stepper::StepRel(long int steps, int axes)
{
    long int i;

    mSpace=mMaxPeriod;

    if (steps>0) {
        for (i=0; i<abs(steps); i++) {
            one_step(true, axes);
            if (mSpace>mMinPeriod)
                mSpace-=mPeriodIncr ;
            if (mSpace<mMinPeriod)
                mSpace=mMinPeriod ;
            if (*pStopFlag) break;
        }
    }
    else if (steps<0) {
        for (i=0; i<abs(steps)+100; i++) {
            one_step(false, axes);
            if (mSpace>mMinPeriod)
                mSpace-=mPeriodIncr;
            if (mSpace<mMinPeriod)
                mSpace=mMinPeriod ;
            if (*pStopFlag) break;
        }
        mSpace=mMaxPeriod;
        for (i=0; i<100; i++) {
            one_step(true, axes);
            if (mSpace>mMinPeriod)
                mSpace-=mPeriodIncr;
            if (mSpace<mMinPeriod)
                mSpace=mMinPeriod ;
            if (*pStopFlag) break;
        }
    };

    emit(Position(
             float(current_pos[X_AX])/10.0,
             float(current_pos[Y_AX])/10.0,
             float(current_pos[Z_AX])/10.0 ));

}

void Stepper::StepAbs(long int steps, int axes)
{
    StepRel(steps-current_pos[axes], axes);
}

long int Stepper::GetPos(int axes)
{
    return current_pos[axes];
}

void Stepper::SetPos(long int newpos, int axes)
{
    current_pos[axes] = newpos;
}

inline void Stepper::usec_delay(long int usecs)
{
// struct timespec req={0};
    struct timespec req;

    req.tv_sec=(long int)(usecs/1000000L);
    req.tv_nsec=1000L*(usecs-1000000L*req.tv_sec);

    while ((nanosleep(&req,&req)==-1) && (errno==EINTR))
        continue;
}

void Stepper::one_step(bool forward, int axes)
{
    static int cnt=0;

    if (axes==X_AX)
        gHal->mfSetXDirBit(forward);
    if (axes==Y_AX)
        gHal->mfSetYDirBit(forward);
    if (axes==Z_AX)
        gHal->mfSetZDirBit(forward);

    if (axes==X_AX)
    {
        usec_delay(mSpace);
        gHal->mfSetXStepBit(true);
        usec_delay(mPulseWidth);
        gHal->mfSetXStepBit(false);
    }
    else if (axes==Y_AX)
    {
        usec_delay(mSpace);
        gHal->mfSetYStepBit(true);
        usec_delay(mPulseWidth);
        gHal->mfSetYStepBit(false);
    }
    else if (axes==Z_AX)
    {
        usec_delay(mSpace);
        gHal->mfSetZStepBit(true);
        usec_delay(mPulseWidth);
        gHal->mfSetZStepBit(false);
    }

    if (forward)
        current_pos[axes]++ ;
    else
        current_pos[axes]-- ;

    if (!((cnt++)%1000)) {
      if (!(current_pos[X_AX]%50) && !(current_pos[Y_AX]%50) && !(current_pos[Z_AX]%50)) {
	emit(Position(
	    float(current_pos[X_AX])/10.0,
	    float(current_pos[Y_AX])/10.0,
	    float(current_pos[Z_AX])/10.0 ));
      }
      qApp->processEvents();
    }
}
